// Copyright 2015-2016 Espressif Systems (Shanghai) PTE LTD
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "app_httpd.h"
#include "esp_http_server.h"
#include "esp_timer.h"
#include "esp_camera.h"
#include "img_converters.h"
#include "fb_gfx.h"
#include "driver/ledc.h"
//#include "camera_index.h"
#include "sdkconfig.h"
#include "app_mdns.h"
#include "app_camera.h"
#include "app_sd.h"
#include "app_http_client.h"

#include "fr_flash.h"
#include "esp_partition.h"
#include "app_board.h" // 2
#include <time.h>      // 2

#if defined(ARDUINO_ARCH_ESP32) && defined(CONFIG_ARDUHAL_ESP_LOG)
#include "esp32-hal-log.h"
#define TAG ""
#else
#include "esp_log.h"
static const char *TAG = "camera_httpd";
#endif

#if CONFIG_ESP_FACE_DETECT_ENABLED

#if CONFIG_ESP_FACE_DETECT_MTMN
#include "fd_forward.h"
#endif

#if CONFIG_ESP_FACE_DETECT_LSSH
#include "lssh_forward.h"
#endif

#if CONFIG_ESP_FACE_RECOGNITION_ENABLED
#include "fr_forward.h"

#define ENROLL_CONFIRM_TIMES 5
#define FACE_ID_SAVE_NUMBER 7
#endif

#define FACE_COLOR_WHITE 0x00FFFFFF
#define FACE_COLOR_BLACK 0x00000000
#define FACE_COLOR_RED 0x000000FF
#define FACE_COLOR_GREEN 0x0000FF00
#define FACE_COLOR_BLUE 0x00FF0000
#define FACE_COLOR_YELLOW (FACE_COLOR_RED | FACE_COLOR_GREEN)
#define FACE_COLOR_CYAN (FACE_COLOR_BLUE | FACE_COLOR_GREEN)
#define FACE_COLOR_PURPLE (FACE_COLOR_BLUE | FACE_COLOR_RED)
#endif

#ifdef CONFIG_LED_ILLUMINATOR_ENABLED
int led_duty = 0;
bool isStreaming = false;
#ifdef CONFIG_LED_LEDC_LOW_SPEED_MODE
#define CONFIG_LED_LEDC_SPEED_MODE LEDC_LOW_SPEED_MODE
#else
#define CONFIG_LED_LEDC_SPEED_MODE LEDC_HIGH_SPEED_MODE
#endif
#endif

typedef struct
{
    httpd_req_t *req;
    size_t len;
} jpg_chunking_t;

#define PART_BOUNDARY "123456789000000000000987654321"
static const char *_STREAM_CONTENT_TYPE = "multipart/x-mixed-replace;boundary=" PART_BOUNDARY;
static const char *_STREAM_BOUNDARY = "\r\n--" PART_BOUNDARY "\r\n";
static const char *_STREAM_PART = "Content-Type: image/jpeg\r\nContent-Length: %u\r\nX-Timestamp: %d.%06d\r\n\r\n";

httpd_handle_t stream_httpd = NULL;
httpd_handle_t camera_httpd = NULL;

#if CONFIG_ESP_FACE_DETECT_ENABLED

static int8_t detection_enabled = 0;

#if CONFIG_ESP_FACE_DETECT_MTMN
static mtmn_config_t mtmn_config = {0};
#endif

#if CONFIG_ESP_FACE_DETECT_LSSH
static lssh_config_t lssh_config;
static int min_face = 80;
#endif

#if CONFIG_ESP_FACE_RECOGNITION_ENABLED
static int8_t recognition_enabled = 0;
static int8_t is_enrolling = 0;
static face_id_list id_list = {0};
#endif

#endif

typedef struct
{
    size_t size;  // number of values used for filtering
    size_t index; // current value index
    size_t count; // value count
    int sum;
    int *values; // array to be filled with values
} ra_filter_t;

static ra_filter_t ra_filter;

static ra_filter_t *ra_filter_init(ra_filter_t *filter, size_t sample_size)
{
    memset(filter, 0, sizeof(ra_filter_t));

    filter->values = (int *)malloc(sample_size * sizeof(int));
    if (!filter->values)
    {
        return NULL;
    }
    memset(filter->values, 0, sample_size * sizeof(int));

    filter->size = sample_size;
    return filter;
}

static int ra_filter_run(ra_filter_t *filter, int value)
{
    if (!filter->values)
    {
        return value;
    }
    filter->sum -= filter->values[filter->index];
    filter->values[filter->index] = value;
    filter->sum += filter->values[filter->index];
    filter->index++;
    filter->index = filter->index % filter->size;
    if (filter->count < filter->size)
    {
        filter->count++;
    }
    return filter->sum / filter->count;
}

#if CONFIG_ESP_FACE_DETECT_ENABLED
#if CONFIG_ESP_FACE_RECOGNITION_ENABLED
static void rgb_print(dl_matrix3du_t *image_matrix, uint32_t color, const char *str)
{
    fb_data_t fb;
    fb.width = image_matrix->w;
    fb.height = image_matrix->h;
    fb.data = image_matrix->item;
    fb.bytes_per_pixel = 3;
    fb.format = FB_BGR888;
    fb_gfx_print(&fb, (fb.width - (strlen(str) * 14)) / 2, 10, color, str);
}

static int rgb_printf(dl_matrix3du_t *image_matrix, uint32_t color, const char *format, ...)
{
    char loc_buf[64];
    char *temp = loc_buf;
    int len;
    va_list arg;
    va_list copy;
    va_start(arg, format);
    va_copy(copy, arg);
    len = vsnprintf(loc_buf, sizeof(loc_buf), format, arg);
    va_end(copy);
    if (len >= sizeof(loc_buf))
    {
        temp = (char *)malloc(len + 1);
        if (temp == NULL)
        {
            return 0;
        }
    }
    vsnprintf(temp, len + 1, format, arg);
    va_end(arg);
    rgb_print(image_matrix, color, temp);
    if (len > 64)
    {
        free(temp);
    }
    return len;
}
#endif
static void draw_face_boxes(dl_matrix3du_t *image_matrix, box_array_t *boxes, int face_id)
{
    int x, y, w, h, i;
    uint32_t color = FACE_COLOR_YELLOW;
    if (face_id < 0)
    {
        color = FACE_COLOR_RED;
    }
    else if (face_id > 0)
    {
        color = FACE_COLOR_GREEN;
    }
    fb_data_t fb;
    fb.width = image_matrix->w;
    fb.height = image_matrix->h;
    fb.data = image_matrix->item;
    fb.bytes_per_pixel = 3;
    fb.format = FB_BGR888;
    for (i = 0; i < boxes->len; i++)
    {
        // rectangle box
        x = (int)boxes->box[i].box_p[0];
        y = (int)boxes->box[i].box_p[1];
        w = (int)boxes->box[i].box_p[2] - x + 1;
        h = (int)boxes->box[i].box_p[3] - y + 1;
        fb_gfx_drawFastHLine(&fb, x, y, w, color);
        fb_gfx_drawFastHLine(&fb, x, y + h - 1, w, color);
        fb_gfx_drawFastVLine(&fb, x, y, h, color);
        fb_gfx_drawFastVLine(&fb, x + w - 1, y, h, color);
#if 0
        // landmark
        int x0, y0, j;
        for (j = 0; j < 10; j+=2) {
            x0 = (int)boxes->landmark[i].landmark_p[j];
            y0 = (int)boxes->landmark[i].landmark_p[j+1];
            fb_gfx_fillRect(&fb, x0, y0, 3, 3, color);
        }
#endif
    }
}
#if 0

static int8_t save_face_id_to_flash_name(face_id_name_list *id_list,char * name )
{
    if(0==l->count){
        ESP_LOGE(TAG, "list is empty");
        return -3;
    }
    // left_sample == 0
    const esp_partition_t *pt = esp_partition_find_first(FR_FLASH_TYPE, FR_FLASH_SUBTYPE, FR_FLASH_PARTITION_NAME);
    if (pt == NULL){
        ESP_LOGE(TAG, "Not found");
        return -2;
    }

    uint8_t enroll_id_idx = l->count - 1;

    // flash can only be erased divided by 4K
    const int block_len = FACE_ID_SIZE * sizeof(float);
    const int block_num = (4096 + block_len - 1) / block_len;

    if(enroll_id_idx % block_num == 0)
    {
        // save the other block TODO: if block != 2
        esp_partition_erase_range(pt, 4096 + enroll_id_idx * block_len, 4096);

        esp_partition_write(pt, 4096 + enroll_id_idx * block_len, l->tail->id_vec->item, block_len);
    }
    else
    {
        // save the other block TODO: if block != 2
        float *backup_buf = (float *)dl_lib_calloc(1, block_len, 0);
        esp_partition_read(pt, 4096 + (enroll_id_idx - 1) * block_len, backup_buf, block_len);

        esp_partition_erase_range(pt, 4096 + (enroll_id_idx - 1) * block_len, 4096);

        esp_partition_write(pt, 4096 + (enroll_id_idx - 1) * block_len, backup_buf, block_len);
        esp_partition_write(pt, 4096 + enroll_id_idx * block_len, l->tail->id_vec->item, block_len); 
        dl_lib_free(backup_buf);
    }

    const int name_len = ENROLL_NAME_LEN * sizeof(char);
    char *backup_name = (char *)dl_lib_calloc(l->count, name_len, 0);
    esp_partition_read(pt, sizeof(int) + sizeof(uint8_t), backup_name, name_len * (l->count - 1));
    memcpy(backup_name + (l->count - 1) * name_len, l->tail->id_name, name_len);
    esp_partition_erase_range(pt, 0, 4096);
    int flash_info_flag = FR_FLASH_INFO_FLAG;
    esp_partition_write(pt, 0, &flash_info_flag, sizeof(int));
    esp_partition_write(pt, sizeof(int), &l->count, sizeof(uint8_t));
    esp_partition_write(pt, sizeof(int) + sizeof(uint8_t), backup_name, name_len * l->count);
    dl_lib_free(backup_name);

    return 0;
    }
static int8_t save_face_id_to_flash(face_id_name_list *id_list)
{
        if(0==id_list->count){
            ESP_LOGE(TAG, "list is empty");
            return -3;
        }
        const esp_partition_t *pt = esp_partition_find_first(FR_FLASH_TYPE, FR_FLASH_SUBTYPE, FR_FLASH_PARTITION_NAME);
        if (pt == NULL){
            ESP_LOGE(TAG, "Not found");
        return -2;
        }

            const int block_len = FACE_ID_SIZE * sizeof(float);
            const int block_num = (4096 + block_len - 1) / block_len;
            float *backup_buf = (float *)dl_lib_calloc(1, block_len, 0);
            int flash_info_flag = FR_FLASH_INFO_FLAG;
            uint8_t enroll_id_idx = id_list->tail == 0 ? (id_list->size - 1) : (id_list->tail - 1) % id_list->size;

            if(enroll_id_idx % block_num == 0)
            {
                // save the other block TODO: if block != 2
                esp_partition_read(pt, 4096 + (enroll_id_idx + 1) * block_len, backup_buf, block_len);

                esp_partition_erase_range(pt, 4096 + enroll_id_idx * block_len, 4096);

                esp_partition_write(pt, 4096 + enroll_id_idx * block_len, id_list->id_list[enroll_id_idx]->item, block_len);
                esp_partition_write(pt, 4096 + (enroll_id_idx + 1) * block_len, backup_buf, block_len); 
            }
            else
            {
                // save the other block TODO: if block != 2
                esp_partition_read(pt, 4096 + (enroll_id_idx - 1) * block_len, backup_buf, block_len);

                esp_partition_erase_range(pt, 4096 + (enroll_id_idx - 1) * block_len, 4096);

                esp_partition_write(pt, 4096 + (enroll_id_idx - 1) * block_len, backup_buf, block_len);
                esp_partition_write(pt, 4096 + enroll_id_idx * block_len, id_list->id_list[enroll_id_idx]->item, block_len); 
            }
            dl_lib_free(backup_buf);

            esp_partition_erase_range(pt, 0, 4096);
            esp_partition_write(pt, 0, &flash_info_flag, sizeof(int));
            esp_partition_write(pt, sizeof(int), id_list, sizeof(face_id_name_list));
        return 0;
}
int8_t e_save_face_id_to_flash(void)
{
        if(0)
            return save_face_id_to_flash_name(&id_list,NULL);
        else        
            return save_face_id_to_flash(&id_list);
}

int8_t e_delete_face_id_in_flash(void)
{
        return delete_face_id_in_flash(&id_list);
}
int8_t e_read_face_id_from_flash(uint8_t sv_id_num,uint8_t cf_times)
{
	uint8_t sv_id_num_t=sv_id_num&0x0f;
	uint8_t cf_times_t=cf_times&0xff;
	
	while(delete_face(&id_list));
	face_id_init(&id_list, sv_id_num_t, cf_times_t);

    	return read_face_id_from_flash(&id_list);
}
#endif
#if CONFIG_ESP_FACE_RECOGNITION_ENABLED
static int run_face_recognition(dl_matrix3du_t *image_matrix, box_array_t *net_boxes)
{
    dl_matrix3du_t *aligned_face = NULL;
    int matched_id = 0;

    aligned_face = dl_matrix3du_alloc(1, FACE_WIDTH, FACE_HEIGHT, 3);
    if (!aligned_face)
    {
        ESP_LOGE(TAG, "Could not allocate face recognition buffer");
        return matched_id;
    }
    if (align_face(net_boxes, image_matrix, aligned_face) == ESP_OK)
    {
        if (is_enrolling == 1)
        {
            int8_t left_sample_face;

            left_sample_face = enroll_face(&id_list, aligned_face);

            if (left_sample_face == (ENROLL_CONFIRM_TIMES - 1))
            {
                ESP_LOGI(TAG, "Enrolling Face ID: %d", id_list.tail); // 2
            }
            ESP_LOGI(TAG, "Enrolling Face ID: %d sample %d", id_list.tail, ENROLL_CONFIRM_TIMES - left_sample_face);
            rgb_printf(image_matrix, FACE_COLOR_CYAN, "ID[%u] Sample[%u]", id_list.tail, ENROLL_CONFIRM_TIMES - left_sample_face);
            if (left_sample_face == 0)
            {
                is_enrolling = 0;
                ESP_LOGI(TAG, "Enrolled Face ID: %d", id_list.tail);
            }
        }
        else
        {
            matched_id = recognize_face(&id_list, aligned_face);
            if (matched_id >= 0)
            {
                ESP_LOGW(TAG, "Match Face ID: %u", matched_id);
                rgb_printf(image_matrix, FACE_COLOR_GREEN, "Hello Subject %u", matched_id);
            }
            else
            {
                ESP_LOGW(TAG, "No Match Found");
                rgb_print(image_matrix, FACE_COLOR_RED, "Intruder Alert!");
                matched_id = -1;
            }
        }
    }
    else
    {
        ESP_LOGW(TAG, "Face Not Aligned");
        // rgb_print(image_matrix, FACE_COLOR_YELLOW, "Human Detected");
    }

    dl_matrix3du_free(aligned_face);
    return matched_id;
}
#endif
#endif

#ifdef CONFIG_LED_ILLUMINATOR_ENABLED
void enable_led(bool en)
{ // Turn LED On or Off
    int duty = en ? led_duty : 0;
    if (en && isStreaming && (led_duty > CONFIG_LED_MAX_INTENSITY))
    {
        duty = CONFIG_LED_MAX_INTENSITY;
    }
    ledc_set_duty(CONFIG_LED_LEDC_SPEED_MODE, CONFIG_LED_LEDC_CHANNEL, duty);
    ledc_update_duty(CONFIG_LED_LEDC_SPEED_MODE, CONFIG_LED_LEDC_CHANNEL);
    ESP_LOGI(TAG, "Set LED intensity to %d", duty);
}
#endif

static size_t jpg_encode_stream(void *arg, size_t index, const void *data, size_t len)
{
    jpg_chunking_t *j = (jpg_chunking_t *)arg;
    if (!index)
    {
        j->len = 0;
    }
    if (httpd_resp_send_chunk(j->req, (const char *)data, len) != ESP_OK)
    {
        return 0;
    }
    j->len += len;
    return len;
}

static esp_err_t capture_handler(httpd_req_t *req)
{
    camera_fb_t *fb = NULL;
    esp_err_t res = ESP_OK;
    int64_t fr_start = esp_timer_get_time();

#ifdef CONFIG_LED_ILLUMINATOR_ENABLED
    enable_led(true);
    vTaskDelay(150 / portTICK_PERIOD_MS); // The LED needs to be turned on ~150ms before the call to esp_camera_fb_get()
    fb = esp_camera_fb_get();             // or it won't be visible in the frame. A better way to do this is needed.
    enable_led(false);
#else
    fb = esp_camera_fb_get();
#endif

    if (!fb)
    {
        ESP_LOGE(TAG, "Camera capture failed");
        httpd_resp_send_500(req);
        return ESP_FAIL;
    }

    httpd_resp_set_type(req, "image/jpeg");
    httpd_resp_set_hdr(req, "Content-Disposition", "inline; filename=capture.jpg");
    httpd_resp_set_hdr(req, "Access-Control-Allow-Origin", "*");

    char ts[32];
    snprintf(ts, 32, "%ld.%06ld", fb->timestamp.tv_sec, fb->timestamp.tv_usec);
    // httpd_resp_set_hdr(req, "X-Timestamp", (const char *)ts);

    size_t fb_len = 0;
    if (fb->format == PIXFORMAT_JPEG)
    {
        fb_len = fb->len;
        res = httpd_resp_send(req, (const char *)fb->buf, fb->len);
    }

    esp_camera_fb_return(fb);
    int64_t fr_end = esp_timer_get_time();
    ESP_LOGI(TAG, "JPG: %uB %ums", (uint32_t)(fb_len), (uint32_t)((fr_end - fr_start) / 1000));

    app_http_client_main((const char *)fb->buf,fb->len);

    return res;
}

static esp_err_t parse_get(httpd_req_t *req, char **obuf)
{
    char *buf = NULL;
    size_t buf_len = 0;

    buf_len = httpd_req_get_url_query_len(req) + 1;
    if (buf_len > 1)
    {
        buf = (char *)malloc(buf_len);
        if (!buf)
        {
            httpd_resp_send_500(req);
            return ESP_FAIL;
        }
        if (httpd_req_get_url_query_str(req, buf, buf_len) == ESP_OK)
        {
            *obuf = buf;
            return ESP_OK;
        }
        free(buf);
    }
    httpd_resp_send_404(req);
    return ESP_FAIL;
}

static int print_reg(char *p, sensor_t *s, uint16_t reg, uint32_t mask)
{
    return sprintf(p, "\"0x%x\":%u,", reg, s->get_reg(s, reg, mask));
}

static int parse_get_var(char *buf, const char *key, int def)
{
    char _int[16];
    if (httpd_query_key_value(buf, key, _int, sizeof(_int)) != ESP_OK)
    {
        return def;
    }
    return atoi(_int);
}

void app_httpd_main()
{
    httpd_config_t config = HTTPD_DEFAULT_CONFIG();
    config.max_uri_handlers = 12;

    httpd_uri_t capture_uri = {
        .uri = "/capture",
        .method = HTTP_GET,
        .handler = capture_handler,
        .user_ctx = NULL};
    ra_filter_init(&ra_filter, 20);

#if CONFIG_ESP_FACE_DETECT_ENABLED

#if CONFIG_ESP_FACE_DETECT_MTMN
    mtmn_config.type = FAST;
    mtmn_config.min_face = 80;
    mtmn_config.pyramid = 0.707;
    mtmn_config.pyramid_times = 4;
    mtmn_config.p_threshold.score = 0.6;
    mtmn_config.p_threshold.nms = 0.7;
    mtmn_config.p_threshold.candidate_number = 20;
    mtmn_config.r_threshold.score = 0.7;
    mtmn_config.r_threshold.nms = 0.7;
    mtmn_config.r_threshold.candidate_number = 10;
    mtmn_config.o_threshold.score = 0.7;
    mtmn_config.o_threshold.nms = 0.7;
    mtmn_config.o_threshold.candidate_number = 1;
#endif

#if CONFIG_ESP_FACE_DETECT_LSSH
    lssh_config = lssh_get_config(min_face, 0.7, 0.3, 240, 320);
#endif

#if CONFIG_ESP_FACE_RECOGNITION_ENABLED
    face_id_init(&id_list, FACE_ID_SAVE_NUMBER, ENROLL_CONFIRM_TIMES);

    // read_face_id_from_flash(&id_list);
#endif

#endif
    ESP_LOGI(TAG, "Starting web server on port: '%d'", config.server_port);
    if (httpd_start(&camera_httpd, &config) == ESP_OK)
    {
        httpd_register_uri_handler(camera_httpd, &capture_uri);
    }

    config.server_port += 1;
    config.ctrl_port += 1;
    ESP_LOGI(TAG, "Starting stream server on port: '%d'", config.server_port);
    if (httpd_start(&stream_httpd, &config) == ESP_OK)
    {
        // httpd_register_uri_handler(stream_httpd, &stream_uri);
    }
}
